#include "MDopt.h"


MDopt::MDopt(void)
{
	Force=0;
	Temp=0;
	Objective=0;

}


MDopt::~MDopt(void)
{
}


void MDopt::optimization(OptData* optData,int maxIter){
	cout<<"initialization"<<endl;
	n=optData->getBeamletNumTotal();

	f1.resize(n,0);
	f2.resize(n,0);
	v1.resize(n,0);
	v2.resize(n,0);
	x1.resize(n,0);
	x2.resize(n,0);
	mass.resize(n,0);
	exf.resize(n,0);

	fstream temFile;

	int htpal;
	double const Vdes=-1;
	int iquench;
	htpal=0; //heat up alogrithm, 0 no heat up,1 velocity rescale
	iquench=1;//0 no quench,1 rapid quench
	double const Bolzk=1;
	dampLambda=0.0005;
	deltaT=0.15;
	maxits = maxIter;

	vector<vector<double> >mmaass; 
	vector<double> mss;
	mss.assign(n,0);
	int massin=0;    //check whether mass has been calculated

	for(int index=0;index!=n;index++)
	{
		x1[index]=(*optData->getWeightTotal())[index];
		mmaass.push_back(mss);
	}
	cout<<"initialdone"<<endl;

	//external Force
	int indexj=0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexj) schedule(dynamic)
#endif
	for(indexj=0;indexj<n;indexj++)		//loop beamlet
	{
		vector<RadiationObj>::iterator iterConstraint;
		for(iterConstraint = optData->getObjectives()->begin();iterConstraint != optData->getObjectives()->end();iterConstraint++)
		{
			//initialization
			string strStructureName = iterConstraint->strStructureName;
			double aimDose = iterConstraint->aimDose;
			double aimWeight = iterConstraint->aimWeight;
			string constraintType = iterConstraint->constraintType;
			vector<vector<double> > *vvDij = optData->getDijTotal();

			vector<int> *aimStructure=NULL;
			for(int indexStr = 0; indexStr != optData->getStructures()->size(); indexStr++)
			{
				if((*optData->getStructures())[indexStr].getStructName() == strStructureName)
				{
					aimStructure = (*optData->getStructures())[indexStr].getPoints();
				}
			}
			if(aimStructure==NULL)
			{
				cerr<<"structure error"<<endl;
			}

			//exf	Cj
			double extTemp;
			extTemp=0;
			for(int indexPoint=0;indexPoint!=aimStructure->size();indexPoint++)	//sum all dij related to tumor
			{
				unsigned int position = (*aimStructure)[indexPoint];
				if((*optData->getDijTotal())[position].size()>indexj){
					extTemp=extTemp+(*optData->getDijTotal())[position][indexj];
				}
			}
			extTemp=extTemp*(2*aimWeight/aimStructure->size())*aimDose;
			exf[indexj]=exf[indexj]+extTemp;
		}
	}
	/*
	temFile.open("exfLL",ios::out);
	for(int ix=0;ix!=n;ix++)
	{temFile<<exf[ix]<<endl;}

	temFile.close();
	temFile.clear();
	cout<<"initialforcedone"<<endl;
	*/
	//mass										//Hjl
//#ifdef USE_OMP
//#pragma omp parallel for
//#endif

	int indexl=0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexj,indexl) schedule(dynamic)
#endif
	for(indexj=0;indexj<n;indexj++)		//loop beamlet
	{
		for(indexl=indexj;indexl!=n;indexl++)
		{
			double totalMass;
			totalMass=0;

			vector<RadiationObj>::iterator iterConstraint;
			for(iterConstraint = optData->getObjectives()->begin();iterConstraint != optData->getObjectives()->end();iterConstraint++)
			{
				//initialization
				string strStructureName = iterConstraint->strStructureName;
				double aimDose = iterConstraint->aimDose;
				double aimWeight = iterConstraint->aimWeight;
				string constraintType = iterConstraint->constraintType;
				vector<vector<double> > *vvDij = optData->getDijTotal();

				vector<int> *aimStructure=NULL;
				for(int indexStr = 0; indexStr != optData->getStructures()->size(); indexStr++)
				{
					if((*optData->getStructures())[indexStr].getStructName() == strStructureName)
					{
						aimStructure = (*optData->getStructures())[indexStr].getPoints();
					}
				}
				if(aimStructure==NULL)
				{
					cerr<<"structure error"<<endl;
				}

				//exf	Cj
				double masTemp;
				masTemp=0;
				for(int indexPoint=0;indexPoint!=aimStructure->size();indexPoint++)	//sum all dij related to tumor
				{
					unsigned int position = (*aimStructure)[indexPoint];
					if((*optData->getDijTotal())[position].size()>indexj){
						masTemp=masTemp+(*optData->getDijTotal())[position][indexj]*(*optData->getDijTotal())[position][indexl];
					}
				}
				masTemp=masTemp*(2*aimWeight/aimStructure->size());
				totalMass = totalMass+masTemp;
			}
			mmaass[indexj][indexl]=totalMass;
			mmaass[indexl][indexj]=totalMass;
		}

		for(int indexl=0;indexl!=n;indexl++)
		{
			mass[indexj]=mmaass[indexj][indexl]+mass[indexj];
		}
	}
	/*
	if(1)
	{

		temFile.open("massmatrixLL",ios::out);
		for(int ix=0;ix!=mmaass.size();ix++)
		{for(int iy=0;iy!=mmaass[ix].size();iy++)
		{temFile<<mmaass[ix][iy];
		if(iy!=(mmaass[ix].size()-1)) temFile<<",";
		}
		temFile<<endl;
		}
		temFile.close();
		temFile.clear();
		temFile.open("massLL",ios::out);
		for(int ix=0;ix!=n;ix++)
		{temFile<<mass[ix]<<endl;}

		temFile.close();
		temFile.clear();
	}
	*/
	//interation begin;
	StopCriteria sc;
	sc.setMaxIter(maxits);
	sc.setEpsnoObj(0.00001);
	sc.setEpsnoSol(0.001);

	//for(int step=0;step<maxits;step++)
	int step=0;
	Objective=0;
	bool stopCondi = false;
	double rho = deltaT;
	//while(!sc.checkStop(resutlWeight,Objective,step))
	while(!stopCondi)
	{	
		
		//calculate force;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexj,indexl) schedule(dynamic)
#endif
		for(indexj=0;indexj<n;indexj++)
		{
			f1[indexj]=0;
			for(indexl=0;indexl<n;indexl++)
			{
				f1[indexj]=f1[indexj]+mmaass[indexj][indexl]*x1[indexl];
			}
			f1[indexj]=(-1.00)*(f1[indexj]-exf[indexj]);
		}

		//calculate new position state.x2
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexj) schedule(dynamic)
#endif
		for(indexj=0;indexj<n;indexj++)
		{
			if(mass[indexj]==0)
			{
				x2[indexj]=0;
			}else{
				x2[indexj]=x1[indexj]+rho*v1[indexj]+rho*rho*f1[indexj]/(2*mass[indexj]);
			}
		}

		//calculate new force state.f2
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexj,indexl) schedule(dynamic)
#endif
		for(indexj=0;indexj<n;indexj++)
		{
			f2[indexj]=0;
			for(indexl=0;indexl<n;indexl++)
			{
				f2[indexj]=f2[indexj]+mmaass[indexj][indexl]*x2[indexl];
			}
			f2[indexj]=(-1.00)*(f2[indexj]-exf[indexj]);
		}

		//calculate new velocity
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexj) schedule(dynamic)
#endif
		for(indexj=0;indexj<n;indexj++)
		{
			v2[indexj]=0;
			if(mass[indexj]==0)
			{
				v2[indexj]=0;
			}else{
				if(htpal==0||step==0) 
				{
					v2[indexj]=v1[indexj]+rho*rho*(f1[indexj]+f2[indexj])/(2*mass[indexj]);
				}else{
					v2[indexj]=sqrt(Vdes/Temp)*v1[indexj]+rho*rho*(f1[indexj]+f2[indexj])/(2*mass[indexj]);
				}
			}
		}
		//positive constrain;
		//for( int iq=0;iq!=n;iq++)
		//{cout<<"x: "<<iq<<" x1: "<<state.x1(iq)<<" v1: "<<state.v1(iq)<<" x2: "<<state.x2(iq)<<" v2: "<<state.v2(iq)<<endl;}
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexj) schedule(dynamic)
#endif
		for(indexj=0;indexj<n;indexj++)
		{
			if(x1[indexj]>=0 && x2[indexj]<0)
			{
				v2[indexj]=(-1)*v2[indexj];
				x2[indexj]=0;
			}
			if(iquench==1)
			{
				if(v2[indexj]*f2[indexj]<0) 
				{
					v2[indexj]=dampLambda*v2[indexj];
				}
			}
			x1[indexj]=x2[indexj];
			v1[indexj]=v2[indexj];

		}
		//calculate the Temperature and Force;
		double Temp=0;
		double Force=0;

#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexj) reduction(+:Temp,Force)
#endif
		for(indexj=0;indexj<n;indexj++)
		{
			Temp += mass[indexj]*v1[indexj]*v1[indexj];
			Force += f1[indexj]*f1[indexj];
		}

		this->Temp=Temp/(3*n*Bolzk);
		this->Force=Force/(n*n);
		//calculate the real object function
		Objective=0;
		int indexCons=0;

		//vector<SConstraint>::iterator iterConstraint;
		//for(iterConstraint = pConstrain->begin();iterConstraint != pConstrain->end();iterConstraint++)
		for(indexCons=0;indexCons<optData->getObjectives()->size();indexCons++)
		{
			RadiationObj iterConstraint = (*optData->getObjectives())[indexCons];
			//initialization
			string strStructureName = iterConstraint.strStructureName;
			double aimDose = iterConstraint.aimDose;
			double aimWeight = iterConstraint.aimWeight;
			string constraintType = iterConstraint.constraintType;
			vector<vector<double> > *vvDij = optData->getDijTotal();

			vector<int> *aimStructure=NULL;
			for(int indexStr = 0; indexStr != optData->getStructures()->size(); indexStr++)
			{
				if((*optData->getStructures())[indexStr].getStructName() == strStructureName)
				{
					aimStructure = (*optData->getStructures())[indexStr].getPoints();
				}
			}
			if(aimStructure==NULL)
			{
				cerr<<"structure error"<<endl;
			}

			//objective
			int indexPoint=0;
			double Objective=0;
#ifdef USE_OMP
#pragma omp parallel for default(shared) private(indexPoint,indexj) reduction(+:Objective)
#endif
			for(indexPoint=0;indexPoint<aimStructure->size();indexPoint++)	//sum all dij related to tumor
			{
				double objTemp;
				objTemp=0;
				for(indexj=0; indexj < n; indexj++)
				{
					unsigned int position = (*aimStructure)[indexPoint];
					if((*optData->getDijTotal())[position].size()>indexj){
						objTemp=objTemp+(*optData->getDijTotal())[position][indexj]*x1[indexj];
					}
				}
				objTemp=(objTemp-aimDose)*(objTemp-aimDose)*(aimWeight/aimStructure->size());
				Objective += objTemp;
			}

			this->Objective += Objective;
		}

		optData->getObjFunValues()->push_back(Objective);
		optData->setWeightTotal(x1);
			
		rho = sc.calculateStepSize(deltaT,optData,step);
		cout<<rho<<"\t";
			
		
		stopCondi = sc.checkStop(optData,Objective,step);
		
		
		step++;

		cout<<"step"<<step<<" "<<Objective<<" "<<Temp<<" "<<Force<<endl;
		temFile<<"step"<<step<<" "<<Objective<<" "<<Temp<<" "<<Force<<endl;
		if(Temp>=Vdes) {htpal=0;iquench=1;}
		if(Temp==0){
			break;
		}
	}

	cout<<"MD optimization finished!"<<endl;
	temFile.close();
	temFile.clear();
}
